/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef _ENVIRONMENT_MEASUREMENT_2D_H_
#define _ENVIRONMENT_MEASUREMENT_2D_H_
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include "TOccupancyGrid.h"

struct TMeasurementPoint{
	public:	
		TMeasurementPoint(){
			x = 0;y = 0;
		}
		TMeasurementPoint(float _x,float _y){
			set(_x,_y);
		}
		void set(float _x,float _y){
			x = _x; y=_y;
		}
		float getDist(){
				return sqrt(x*x+y*y);
		}
		float getAngle(){
				return atan2(y,x);
		}
		float x;
		float y;
};
/**
* Header for measurement class. Contains info about origin and its uncertainity
* with respect to localized map.
*/
struct TMeasurementHeader{
	public:
		TMeasurementHeader(){
			setFrame(0,0,0); setError(0,0,0);
		}
		void setFrame(float x,float y,float a){
			x_ref=x;y_ref=y;a_ref=a;
		}
		void setError(float x,float y,float a){
			x_err=x;y_err=y;a_err=a;
		}
		void print(){
			fprintf(stderr,"(%.2f,%.2f,%.2f) (%.2f,%.2f,%.2f)\n", x_ref,y_ref,a_ref,x_err,y_err,a_err);
		}
		
		void fprint(FILE *f){
			fprintf(f,"%.2f %.2f %.2f %.2f %.2f %.2f ", x_ref,y_ref,a_ref,x_err,y_err,a_err);
		}
		void fread(FILE *f){
			fscanf(f,"%f %f %f %f %f %f ",&x_ref,&y_ref,&a_ref,&x_err,&y_err,&a_err);
		}
		
		float x_ref, y_ref, a_ref;      ///< The frame for the measurement points
		float x_err, y_err, a_err;      ///< The "error" of the frame (the true frame must fit into (x_ref +- x_err,...))
};

struct TTrajectory{
		std::vector<Grid::pose> traj;
		
		bool fread(FILE *f){
				if(f==NULL){
						fprintf(stderr,"TTrajectory::fread: Input file ptr is NULL\n");
						return false;
				}
				traj.clear();
				
				Grid::pose p;
				char c=0;
				while( (c!='\n')  ){
						if(feof(f)) break;
						fscanf(f,"%f %f %f",&p.x,&p.y,&p.a);
						fscanf(f,"%c",&c);
						traj.push_back(p);
				}
				return true;
		}
		bool fprint(FILE *f){
				if(f==NULL){
						fprintf(stderr,"TTrajectory::fprint: Output file ptr is NULL\n");
						return false;
				}
				if(traj.size()<1){
						fprintf(stderr,"TTrajectory::fprint:Invalid number of points!!!\n");
						return false;
				} 
				for(unsigned int i=0;i<traj.size()-1;i++){
						fprintf(f,"%.3f %.3f %.3f ",traj[i].x,traj[i].y,traj[i].a);
				}
				fprintf(f,"%.3f %.3f %3.f\n",traj[traj.size()-1].x,traj[traj.size()-1].y,traj[traj.size()-1].a);
				return true;
		}
		unsigned int size(){return traj.size();}
		
};

/**
* defines for net packets
*/
#define ENV_MEASUREMENT_HEADER_SIZE (4*6)
#define MEASUREMENT_POINTS_SIZE (4*2)

class CEnvironmentMeasurement2D{
	public:
		/**
		* Default constructor
		*/
		CEnvironmentMeasurement2D();
		/**
		 * Constructor for net packet
		 **/
		CEnvironmentMeasurement2D(const char *in,unsigned int data);
		
		/**
		 * Constructor for Occupance grid
		 **/
		CEnvironmentMeasurement2D(TOccupancyGrid *g, float th);
		
		/**
		 * Constructor for FILE input
		 **/
		CEnvironmentMeasurement2D(FILE *f);
		
		/**
		* Clears the points 
		*/
		void clear(){points.clear();}
		/**
		* Add point to whole measurement
		* @param &p reference to measurement point structure
		*/
		void addPoint(TMeasurementPoint &p){points.push_back(p);}
		/**
		* returns the number of points 
		**/
		unsigned int size(){return points.size();}
		
		/**
		* Returns a "block" of data which contains the content of the class
		* @param **out pointer to a pointer where the data will be put tu. If NULL, memory is reserved
		* @returns number of bytes
		**/
		unsigned int getNetPacket(char **out);
		
		/**
		* Creates the class from a block of data.
		* @param *in a pointer to data generated by a "getNetPacket" method
		* @param size the number of bytes in *in
		* @return true if success
		**/
		bool setNetPacket(const char *in, unsigned int size);
		
		/**
		* Sets an occupancy grid map to points vector. 
		* The grid cells are intepreted as measurement if the value is grater than th.
		* @param *g pointer to an occupancy grid
		* @param th threshold for choosing measurement points
		*/
		bool setOccupancyGrid(TOccupancyGrid *g, float th);
		
		void print();
		/**
		* output to file
		*/
		void fprint(FILE *f);
		
		/**
		 * input from file
		 */
		void fread(FILE *f);
		
		std::vector<TMeasurementPoint> points; ///< All measurement points
		TMeasurementHeader frame;              ///< Frame info 
		
	private:
		
		
};


#endif

